#include "bsp.h"
#include "protocol.h"

#define speed_go 300 //³öÍ£Ö¹ÏßËÙ¶È Stop line speed
#define de_time  200  //³öÍ£Ö¹ÏßÑÓ³ÙÊ±¼ä Exit stop line delay time

uint8_t stop_flag = 0;
uint32_t n_stime = 0;   //»Ö¸´Õý³£Ñ²ÏßµÄÊ±¼ä Time to resume normal line patrol
uint16_t wait_k210 = 700;//µÈ´ýk210Æô¶¯µÄÊ±¼ä Waiting time for k210 to start
uint8_t id_num = MAX_id;
uint8_t id_num_back = MAX_id;
extern uint16_t g_car_time;

// 发送当前版本号到主机上
void Bsp_Send_Version(void)
{
	#define LEN       7
	uint8_t data[LEN] = {0};
	uint8_t checknum = 0;
	data[0] = PTO_HEAD;
	data[1] = PTO_DEVICE_ID - 1;
	data[2] = LEN - 2;           // 数量
	data[3] = FUNC_VERSION;   // 功能字
	data[4] = VERSION_MAJOR;      // 大版本号, 组成结果：1.2
	data[5] = VERSION_MINOR;      // 小版本号

	for (uint8_t i = 2; i < LEN - 1; i++)
	{
		checknum += data[i];
	}
	data[LEN - 1] = checknum;
	USART1_DataString(data, LEN);
}

void BSP_Init(void)
{
	Delay_Init();
	USART1_UART_Init();
	//printf("Smart32 uart1 Init\r\n");
    return;
	OLED_Init();
	OLED_Draw_Line("oled init success!", 1, true, true);
	printf("oled Init\r\n");

	// Serial port initialization MPU6050-GPIO初始化
	MPU6050_GPIO_Init();   // PB10 PB11

	// MPU self check initialization MPU自检初始化
	MPU6050_Init();
	printf("MPU6050 Init\r\n");
	Bsp_Tim_Init();
	PID_Param_Init();//??PID???
	
	USART2_UART_Init();
	printf("usart2 Init\r\n");
}

// Loop Run Function
// Parameter:None
void BSP_Loop(void)
{
	// Print mpu data 打印mpu数据
	MPU6050_Pose();
	car_data_t car_data;
	Motion_Get_Speed(&car_data);
	printf("car->Vx = %d\t car->Vy = %d\t car->Vz = %d\t \r\n", car_data.Vx, car_data.Vy, car_data.Vz);
		Irrmote_car();
	    if(g_car_time==0)
	    {
		    wheel_State(MOTION_STOP,0);
	    }
	HAL_Delay(200); // ms
	user_lightseek_detect();
}
